#include "fisheye.hpp"
#include "coarseLocating.hpp"
#include "INIReader.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include "System.h"

using namespace cv;
using namespace std;
using namespace xyVision;

xyVision::coarseLocating::coarseLocating(const string& configName) {
    this->setParameters(configName);
    this->isDetected = false;

    cameraInfo.newCameraMatrix = Matx33f(
        cameraInfo.cameraMatrix(0,0)*this->scale, 0.0f, cameraInfo.newSize.width/2.0f,
        0.0f, cameraInfo.cameraMatrix(0,0)*this->scale, cameraInfo.newSize.height/2.0f,
        0.0f, 0.0f, 1.0f);
    xyVision::fisheye::initUndistortRectifyMap(cameraInfo.cameraMatrix, cameraInfo.distortCoeff,
            cv::Matx33d::eye(), cameraInfo.newCameraMatrix, cameraInfo.newSize, 
            CV_16SC2, cameraInfo.map1, cameraInfo.map2);
}

void xyVision::coarseLocating::setParameters(const string& configName) {
    INIReader reader(configName);
    string calibConfigName;
    const char* section = "cameraCamConf";
    const char* key = "calibPath";
    calibConfigName = reader.Get(section, key, " ");
    cv::FileStorage fs(calibConfigName, cv::FileStorage::READ);

    Mat temp;
    fs["camera_matrix"] >> temp;
    cameraInfo.cameraMatrix = Matx33f(temp);
    fs["distortion_coefficients"] >> temp;
    temp = temp.t();
    cameraInfo.distortCoeff = Matx14f(temp);

    key = "imageWidth";
    int width = reader.GetInteger(section, key, -1);
    key = "imageHeight";
    int height = reader.GetInteger(section, key, -1);
    cameraInfo.imageSize = Size(width, height);

    section = "SLAM";
    key = "voc_file";
    string voc_file = reader.Get(section, key, " ");
    if (voc_file != " ")
    {
        section = "SLAM";
        key = "voc_file";
        slamInfo.voc_file = reader.Get(section, key, " ");
        key = "settings_file";
        slamInfo.settings_file = reader.Get(section, key, " ");
        key = "map_file";
        slamInfo.map_file = reader.Get(section, key, " ");
        this->SLAM = new ORB_SLAM2::System(slamInfo.voc_file, slamInfo.settings_file,
            ORB_SLAM2::System::MONOCULAR, false, true, true, slamInfo.map_file);
        slamInfo.ts = 0.;
        section = "SLAM";
        key = "ts_interval";
        slamInfo.ts_interval = reader.GetReal(section, key, 0.2);
    }

    section = "processConf";
    key = "scale";
    this->scale = (float)reader.GetReal(section, key, -1.0);
    this->cameraInfo.newSize = Size(int(width*this->scale), 
        int(height*this->scale));
}

void xyVision::coarseLocating::getCurrentGPS(const Mat& image) {
    this->oriImg = image.clone();
    this->adjustImg(image, this->rectifiedImg);

    // run SLAM and compute reference frame
    this->getGPS();
}

void xyVision::coarseLocating::adjustImg(const Mat& input, Mat& output)
{
    cv::remap(input, output, this->cameraInfo.map1, this->cameraInfo.map2,
        INTER_LINEAR, BORDER_CONSTANT);
}

void xyVision::coarseLocating::getGPS() {
    Mat Tcw = this->SLAM->TrackMonocular(this->rectifiedImg, this->slamInfo.ts);
    ORB_SLAM2::Tracking* tr = SLAM->GetTracker();
    slamInfo.ts += slamInfo.ts_interval;

    if (tr->mState == ORB_SLAM2::Tracking::OK) {
        // get refrence key frame
        ORB_SLAM2::KeyFrame* kf = tr->getCurrentKeyFrame();
        if ((int)kf->mgps.size() == 2) {
            this->gps = kf->mgps;
            this->isDetected = true;
        } else {
            this->isDetected = false;
            this->gps.resize(2);
            this->gps[0] = 0;
            this->gps[1] = 0;

        }
        
    }
    else {
        this->isDetected = false;
        this->gps.resize(2);
        this->gps[0] = 0;
        this->gps[1] = 0;
    }
}



